#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "../drv/drv_i2c.h"
#include "hmc5883l.h"


#define PI       3.14159265358979323846
#define HMC5883_ADDRESS 	0x1E 
#define SENSIVITY_HMC5883L_GAUSS_1_3                1090

//calibration data
S_Mag mag_calib;
int mag_calibrate = 0;

void HMC5883LInit( int fd )
{

	printf("HMC5883L Init\n");
	drv_i2cSetDevice(fd, HMC5883_ADDRESS);
	drv_i2cWriteByte(fd, HMC5883_CONFIG_A, HMC5883_75HZ);
	drv_i2cWriteByte(fd, HMC5883_CONFIG_B,HMC5883_1_3GA);
	drv_i2cWriteByte(fd, HMC5883_MODE,HMC5883_CONTINUOUS);

	mag_calib.x = 0;
	mag_calib.y = 0;
	mag_calib.z = 0;

	mag_calibrate = 0;
}	


//Mag X Y Z
void HMC5883LRead( int fd, S_Mag *mag)
{	
	drv_i2cSetDevice(fd, HMC5883_ADDRESS);
	mag->x = drv_i2cReadShort(fd, HMC5883_X_MSB);
	mag->y = drv_i2cReadShort(fd, HMC5883_Y_MSB);
	mag->z = drv_i2cReadShort(fd, HMC5883_Z_MSB);
	
	if(mag_calibrate == 1)
	{
		mag->x = mag->x - mag_calib.x;
		mag->y = mag->y - mag_calib.y;
		mag->z = mag->z - mag_calib.z;

		mag->x = mag->x / SENSIVITY_HMC5883L_GAUSS_1_3;
		mag->y = mag->y / SENSIVITY_HMC5883L_GAUSS_1_3;
		mag->z = mag->z / SENSIVITY_HMC5883L_GAUSS_1_3;
	}
}



void HMC5883LCalibrate( int fd )
{
	printf("HMC5883L Calibrate...");
	fflush(stdout);
	float mag_sum[ 3 ] = {0,0,0};
	S_Mag mag_read;
	int i;
	int div = 20;
	for(i=0 ; i < div ; i++)
	{
		HMC5883LRead(fd , &mag_read);
		mag_sum[ 0 ] = mag_sum[ 0 ] + mag_read.x;
		mag_sum[ 1 ] = mag_sum[ 1 ] + mag_read.y;
		mag_sum[ 2 ] = mag_sum[ 2 ] + mag_read.z;
	}
	mag_calib.x = (float)(mag_sum[ 0 ] / div);
	mag_calib.y = (float)(mag_sum[ 1 ] / div);
	mag_calib.z = (float)(mag_sum[ 2 ] / div);


	mag_calibrate = 1;
	printf("\rHMC5883L Calibrate   \n");
}








float CalculateHeadingTiltCompensated(S_Mag mag, S_Mag acc)
{
	// We are swapping the accelerometers axis as they are opposite to the compass the way we have them mounted.
	// We are swapping the signs axis as they are opposite.
	// Configure this for your setup.
	float accX = -acc.y;
	float accY = -acc.x;

	float rollRadians = asin(accY);
	float pitchRadians = asin(accX);

	// We cannot correct for tilt over 40 degrees with this algorthem, if the board is tilted as such, return 0.
	if(rollRadians > 0.78 || rollRadians < -0.78 || pitchRadians > 0.78 || pitchRadians < -0.78)
	{
		return 0;
	}

	// Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.
	float cosRoll = cos(rollRadians);
	float sinRoll = sin(rollRadians);  
	float cosPitch = cos(pitchRadians);
	float sinPitch = sin(pitchRadians);

	// The tilt compensation algorithem.
	float Xh = mag.x * cosPitch + mag.z * sinPitch;
	float Yh = mag.x * sinRoll * sinPitch + mag.y * cosRoll - mag.z * sinRoll * cosPitch;

	float declinationAngle = 18.04 / 1000.0;
	float heading = atan2(Yh, Xh) - declinationAngle;

	printf("%f heading\n", heading);
	return heading;


}

			


